﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Casanova.Math;

namespace StudentsAssignment
{
    class PathWorld
    {
        public List<PathNode> m_nodes;

        public PathFinder m_PathFinder = new PathFinder();

		void CreateWorld()
        {
            m_nodes = new List<PathNode>();
	
            /*
            for( int i = 0; i < MAX_NUM_NODES; ++i )
            {
	            m_nodes[i].category = path_node_category.UNVISITED;
	            m_nodes[i].connection = NULL;
	            m_nodes[i].cost_so_far = 0;
	            m_nodes[i].estimated_cost = 0;
	            m_nodes[i].is_target = false;
	            m_nodes[i].m_num_links = 0;
            }
             * */
        }
		
		void AddNode( float pos_x, float pos_z )
        {
            int m_num_nodes = m_nodes.Count;
            m_nodes.Add(new PathNode());
            m_nodes[m_num_nodes].pos = new Vector2( pos_x, pos_z );

            ++m_num_nodes;
        }


        int FindNearestNode(float pos_x, float pos_y)
        {
            int path_start_id = 0;
		
		    float min_dist = 0xFFFFFF;
		
		    Vector2 start_pos = new Vector2(pos_x, pos_y );
		
		    // search for the nearest node to the starting point
		    for( int i = 0; i < m_nodes.Count; ++i ) {

                float dist = (m_nodes[i].pos - start_pos).Length;

			    if( dist < min_dist ) {
				    min_dist = dist;
				    path_start_id = i;
			    }
		    }
		
		    return path_start_id;
        }

        PathNode GetNode(int id)
        {
            return m_nodes[id];
        }

        public int FindPath(
        float start_x,
        float start_y,
        float target_x,
        float target_y,
        ref List<PathNode> out_path,
        int out_max_length,
        ref int path_length)
        {
            // todo pick nearest node on the path to the start/end position
		int path_start_id = FindNearestNode( start_x, start_y );
		int path_end_id = FindNearestNode( target_x, target_y );
		
		// we are already at the target
		if( path_start_id == path_end_id ) {
			out_path.Clear();
			path_length = 0;
			return 1;
		}
		
		m_nodes[path_start_id].category = path_node_category.OPEN;
		m_nodes[path_start_id].cost_so_far = 0;
		m_nodes[path_start_id].estimated_cost = estimate_cost( path_start_id, path_end_id );
		m_nodes[path_start_id].is_target = true;
		
		m_nodes[path_end_id].is_target = true;
		
		// and add it to the open list
        m_PathFinder.open_list_add( m_nodes[path_start_id] );
		
		while( m_PathFinder.open_list_empty() == false )
		{
			// get the node with the smalles estimate from the open list
			PathNode current = m_PathFinder.open_list_top();

            int current_id = FindNode(current.pos); // m_nodes.BinarySearch(current);
			//u32 current_x = current_id % SCRWIDTH;
			//u32 current_y = current_id / SCRWIDTH;
			
			if( current == m_nodes[path_end_id] ) break; // we have reached the end
			
			m_PathFinder.open_list_remove_top();
			
			// now check all nodes connected
			for( int i = 0; i < current.m_links.Count; ++i )
			{
				int connect_id = current.m_links[i].id;
				
				PathNode connect = m_nodes[connect_id];
				
				if( connect.category == path_node_category.UNVISITED )
				{
					// we have not found this node before so we need to calculate all values
					connect.cost_so_far = current.cost_so_far + estimate_cost( current_id, connect_id );
					connect.estimated_cost = estimate_cost( connect_id, path_end_id );
					connect.category = path_node_category.OPEN;
					connect.connection = current;
					
					// and add it to the open list
					m_PathFinder.open_list_add( connect );
				} 
				else if( connect.category == path_node_category.OPEN )
				{
					float new_cost_so_far = current.cost_so_far + estimate_cost( current_id, connect_id );
					
					if( connect.cost_so_far <= new_cost_so_far ) continue;
					
					// update the node
					connect.cost_so_far = new_cost_so_far;
					connect.estimated_cost = estimate_cost( connect_id, path_end_id );
					connect.category = path_node_category.OPEN;
					connect.connection = current;
				}
				else if( connect.category == path_node_category.CLOSED )
				{
					float new_cost_so_far = current.cost_so_far + estimate_cost( current_id, connect_id );
					
					if( connect.cost_so_far <= new_cost_so_far ) continue;
					
					// update the node
					connect.cost_so_far = new_cost_so_far;
					connect.estimated_cost = estimate_cost( connect_id, path_end_id );
					connect.category = path_node_category.OPEN;
					connect.connection = current;
					
					m_PathFinder.open_list_add( connect );
				}
			}
			
			current.category = path_node_category.CLOSED;
		}
		
		m_nodes[path_start_id].is_target = false;
		m_nodes[path_end_id].is_target = false;
		
		// and reset everything to unvisited
		for( int i = 0; i < m_nodes.Count; ++i )
		{
			m_nodes[i].category = path_node_category.UNVISITED;
		}
		
		// check if we have found a path
		PathNode current_ = m_PathFinder.open_list_top();
		
		if( current_ != m_nodes[path_end_id] )
		{
			m_PathFinder.open_list_clear();
			return 1;
		}
		
		int depth = 0;
		
		// otherwise firstly get the depth
		while( current_ != m_nodes[path_start_id] )
		{
			current_ = current_.connection;
			++depth;
		}
		
		++depth;
		
		if( depth > out_max_length )
		{
			Console.WriteLine( "the path is longer than supported, please increase out_max_length\n" );
		}
		
		// set the last to null so that we don't need to save the length of the path
		// out_path[depth] = NULL;

        out_path = new List<PathNode>(depth);
        for (int i = 0; i < depth; ++i)
        {
            out_path.Add(new PathNode());
        }
		path_length = (depth-1);
		
		current_ = m_PathFinder.open_list_top();
		while( current_ != m_nodes[path_start_id] )
		{
			--depth;
			out_path[depth] = current_;
			current_ = current_.connection;
		}
		
		out_path[0] = m_nodes[path_start_id];

        m_PathFinder.open_list_clear();
		
		return 0;
        }

        float estimate_cost(int start_id, int end_id)
        {
            return (m_nodes[end_id].pos - m_nodes[start_id].pos).Length;
        }

        internal void CreateWorld(VirtualCity.CityDescription.WalkGraph walk_graph)
        {
            m_nodes = new List<PathNode>();

            foreach (var x in walk_graph.Links)
            {
                int id_first = FindNode( x.Item1.Position );
                int id_last = FindNode(x.Item2.Position);

                m_nodes[id_last].AddLink(id_first);
                m_nodes[id_first].AddLink(id_last);
            }

            // walk_graph.Links.Length;
        }

        private int FindNode(Vector2 vector2)
        {
            for (int i = 0; i < m_nodes.Count; ++i)
            {
                if ( (int)m_nodes[i].pos.X == (int)vector2.X && (int)m_nodes[i].pos.Y == (int)vector2.Y)
                {
                    return i;
                }
            }

            int new_id = m_nodes.Count;

            PathNode node = new PathNode();
            node.pos = vector2;
            node.category = path_node_category.UNVISITED;
            node.connection = null;
            node.cost_so_far = 0;
            node.estimated_cost = 0;
            node.is_target = false;
            node.m_links = new List<PathLink>();

            m_nodes.Add(node);

            return new_id;
        }
    }
}
